#include "CBezier.h"
#include <cmath>
#include <QDebug>

CBezier::CBezier(QObject *parent) : QObject(parent)
{
    m_origin_points.clear();
    m_order = 2;
}

void CBezier::bezierFitting()
{
    switch (m_order) {
    case 1:

        break;
    case 2:
        twoOrderBezier();
        break;
    case 3:
        threeOrderBezier();
        break;
    default:
        break;
    }

}

void CBezier::twoOrderBezier()
{
#if 0
    for (float t = 0.0; t <= 1.0; t += 0.01)
    {
        QPoint p1, p2, p3;

        int p1_x = calcTwoOrderBezierX(m_origin_points[0], m_origin_points[1], t);
        int p1_y = calcTwoOrderBezierY(m_origin_points[0], m_origin_points[1], t);
        p1.setX(p1_x);
        p1.setY(p1_y);

        int p2_x = calcTwoOrderBezierX(m_origin_points[1], m_origin_points[2], t);
        int p2_y = calcTwoOrderBezierY(m_origin_points[1], m_origin_points[2], t);
        p2.setX(p2_x);
        p2.setY(p2_y);

        int p3_x = calcTwoOrderBezierX(p1, p2, t);
        int p3_y = calcTwoOrderBezierY(p1, p2, t);
        p3.setX(p3_x);
        p3.setY(p3_y);

        emit signalTwoOrderRollLine(p1, p2);
        emit signalSendTwoBezierPoint(p3);

        QThread::msleep(100);
    }
#else

    m_start_point = m_origin_points[0];
    for (int i = 1; i < m_origin_points.size() - 1; i ++)
    {
        m_key_point = m_origin_points[i];
        QPoint middle;
        middle.setX((m_origin_points[i + 1].x() + m_origin_points[i].x())/2);
        middle.setY((m_origin_points[i + 1].y() + m_origin_points[i].y())/2);

        if (i == m_origin_points.size() - 2)
        {
            m_end_point = m_origin_points.back();
        }
        else
        {
            m_end_point = middle;
        }

        twoOrderBezierRecursion(m_start_point, m_key_point, m_end_point, 0.0);
        m_start_point = m_end_point;
    }
    qDebug("end\n");
#endif
}

void CBezier::threeOrderBezier()
{
    m_start_point = m_origin_points[0];
    for (int i = 1; i < m_origin_points.size() - 2; i +=2)
    {
        QPoint key_1 = m_origin_points[i];
        QPoint key_2 = m_origin_points[i + 1];

        QPoint middle;
        middle.setX((m_origin_points[i + 2].x() + m_origin_points[i + 1].x())/2);
        middle.setY((m_origin_points[i + 2].y() + m_origin_points[i + 1].y())/2);

        if (i >= m_origin_points.size() - 3)
        {
            m_end_point = m_origin_points.back();
        }
        else
        {
            m_end_point = middle;
        }

        threeOrderBezierRecursion(m_start_point, key_1, key_2, m_end_point, 0.0);
        m_start_point = m_end_point;
    }

}

void CBezier::twoOrderBezierRecursion(QPoint point1, QPoint point2, QPoint point3, float t)
{
    if (1.0 - t < 0.001)
    {
        return;
    }

    QPoint p1, p2, p3;

    int p1_x = calcBezierX(point1, point2, t);
    int p1_y = calcBezierY(point1, point2, t);
    p1.setX(p1_x);
    p1.setY(p1_y);

    int p2_x = calcBezierX(point2, point3, t);
    int p2_y = calcBezierY(point2, point3, t);
    p2.setX(p2_x);
    p2.setY(p2_y);

    int p3_x = calcBezierX(p1, p2, t);
    int p3_y = calcBezierY(p1, p2, t);
    p3.setX(p3_x);
    p3.setY(p3_y);

    emit signalTwoOrderRollLine(p1, p2);
    emit signalSendTwoBezierPoint(p3);

    QThread::msleep(100);

    t += 0.05;
    twoOrderBezierRecursion(point1, point2, point3, t);
}

void CBezier::threeOrderBezierRecursion(QPoint point1, QPoint point2, QPoint point3, QPoint point4, float t)
{
    if (1.0 - t < 0.001)
    {
        return;
    }

    QPoint p1, p2, p3, p4, p5, p6;

    int p1_x = calcBezierX(point1, point2, t);
    int p1_y = calcBezierY(point1, point2, t);
    p1.setX(p1_x);
    p1.setY(p1_y);

    int p2_x = calcBezierX(point2, point3, t);
    int p2_y = calcBezierY(point2, point3, t);
    p2.setX(p2_x);
    p2.setY(p2_y);

    int p3_x = calcBezierX(point3, point4, t);
    int p3_y = calcBezierY(point3, point4, t);
    p3.setX(p3_x);
    p3.setY(p3_y);

    int p4_x = calcBezierX(p1, p2, t);
    int p4_y = calcBezierY(p1, p2, t);
    p4.setX(p4_x);
    p4.setY(p4_y);

    int p5_x = calcBezierX(p2, p3, t);
    int p5_y = calcBezierY(p2, p3, t);
    p5.setX(p5_x);
    p5.setY(p5_y);

    int p6_x = calcBezierX(p4, p5, t);
    int p6_y = calcBezierY(p4, p5, t);
    p6.setX(p6_x);
    p6.setY(p6_y);

    emit signalThreeOrderRollLine(p1, p2, p2, p3, p4, p5);
    emit signalSendThreeBezierPoint(p6);

    QThread::msleep(100);

    t += 0.05;
    threeOrderBezierRecursion(point1, point2, point3, point4, t);
}

int CBezier::calcBezierX(QPoint point_1, QPoint point_2, float t)
{
    return static_cast<int>((1 - t) * point_1.x() + t * point_2.x());
}

int CBezier::calcBezierY(QPoint point_1, QPoint point_2, float t)
{
    return static_cast<int>((1 - t) * point_1.y() + t * point_2.y());
}
